#include "zino.h"
#include "gyro.h"
#include "lsm6dsr_reg.h"
#include "lsm6dsr_spi2.h"
#include "maths.h"
#include "zino_config.h"
#include "rtthread.h"
// #include "shanwai.h"

#define DBG_TAG "zGyro"
#define DBG_LVL DBG_LOG
#define LSM6DSR_USE_SPI
// #define LSM6DSR_USE_I2C
#include <rtdbg.h>

int zino_gyro_init(struct gyro_device *gyro)
{
    gyro->variance[0].buffer = gyro->x_traceBuffer;
    gyro->variance[0].len = 64;
    gyro->variance[1].buffer = gyro->y_traceBuffer;
    gyro->variance[1].len = 64;
    gyro->variance[2].buffer = gyro->z_traceBuffer;
    gyro->variance[2].len = 64;
    gyro->scale_factor = lsm6dsr_from_fs500dps_to_mdps(1) * 0.001f;
    gyro->calibrated_axis = 0x00;
    gyro->offset.x = 0;
    gyro->offset.y = 0;
    gyro->offset.z = 0;
    return lsm6dsr_spi2_read_data_polling_int();
}
int zino_gyro_update(struct gyro_device *dev)
{
    int ret = dev->get_raw_data(dev);
    for (uint8_t i = 0; i < 3; i++)
    {
        if (dev->calibrated_axis != 0x07)
        {
            variance_calculate(&dev->variance[i], dev->adc.raw[i]);
            if (dev->variance[i].variance < 50 && dev->variance[i].ready)
            {
                dev->calibrated_axis |= (1 << i);
                dev->offset.raw[i] = dev->variance[i].average;
            }
            else
            {
                dev->calibrated_axis &= ~(1 << i);
            }
            // if (i == 2)
            // {
            //     rt_kprintf("gyro:%4d, %4d, %4d ", dev->adc.x, dev->adc.y, dev->adc.z);
            //     rt_kprintf("var:%4d, %4d, %4d ", dev->variance[0].variance, dev->variance[1].variance, dev->variance[2].variance);
            //     rt_kprintf("ave:%4d, %4d, %4d ", dev->variance[0].average, dev->variance[1].average, dev->variance[2].average);
            //     rt_kprintf("sum: %4d %4d %4d %02X\n", dev->variance[0].sum, dev->variance[1].sum, dev->variance[2].sum, dev->calibrated_axis);
            // }
            if (dev->calibrated_axis == 0x07)
            {
                // dev->offset.raw[0] = dev->variance[0].average;
                // dev->offset.raw[1] = dev->variance[1].average;
                // dev->offset.raw[2] = dev->variance[2].average;
                LOG_D("offset[%d]: %d,%d,%d", i, dev->offset.raw[0], dev->offset.raw[1], dev->offset.raw[2]);
            }
        }
        dev->adc.raw[i] = (ABS(dev->adc.raw[i]) > 10) ? dev->adc.raw[i] : 0;//deadband
        dev->value.raw[i] = (dev->adc.raw[i] - dev->offset.raw[i]) * dev->scale_factor;
    }
    return ret;
}

int zino_gyro_get_raw_data(struct gyro_device *gyro)
{
    int16_t buffer[3];
    int ret = lsm6dsr_spi2_read_gyro_adc_data(buffer);
    // LOG_D("buffer:%d,\t %d, \t %d",buffer[0],buffer[1],buffer[2]);
    gyro->adc.y = -buffer[0];
    gyro->adc.x = -buffer[1];
    gyro->adc.z = -buffer[2];
    return ret;
}
struct gyro_device zGyroDev = {
    .init = zino_gyro_init,
    .get_raw_data = zino_gyro_get_raw_data,
    .update = zino_gyro_update,
};

void zino_gyro_task(void *p)
{
    struct gyro_device *dev = &zGyroDev;
    dev->init(dev);
    while (1)
    {
        dev->get_raw_data(dev);
        for (uint8_t i = 0; i < 3; i++)
        {
            if (dev->calibrated_axis != 0x07)
            {
                variance_calculate(&dev->variance[i], dev->adc.raw[i]);
                if (dev->variance[i].variance < 30 && dev->variance[i].ready)
                {
                    dev->calibrated_axis |= (1 << i);
                }
                else
                {
                    dev->calibrated_axis &= ~(1 << i);
                }
                if (dev->calibrated_axis == 0x07)
                {
                    dev->offset.raw[i] = dev->variance[i].average;
                    LOG_D("offset[%d]: %d,%d,%d", i, dev->offset.raw[0], dev->offset.raw[1], dev->offset.raw[2]);
                }
                if (i == 2)
                {
                    rt_kprintf("gyro x_var:%4d, y_var:%4d, z_var:%4d \n", dev->variance[0].variance, dev->variance[1].variance, dev->variance[2].variance);
                    rt_kprintf("gyro x:%4d, y:%4d, z:%4d ", dev->adc.x, dev->adc.y, dev->adc.z);
                    rt_kprintf("average x:%4d, y:%4d, z:%4d ", dev->variance[0].average, dev->variance[1].average, dev->variance[2].average);
                    rt_kprintf("sum: %4d %4d %4d %02X\n", dev->variance[0].sum, dev->variance[1].sum, dev->variance[2].sum, dev->calibrated_axis);
                }
            }
            dev->value.raw[i] = (dev->adc.raw[i] - dev->offset.raw[i]) * dev->scale_factor;
        }

        // digitalOsc_Head();
        // digitalOsc_float(dev->value.x);
        // digitalOsc_float(dev->value.y);
        // digitalOsc_float(dev->value.z);
        // digitalOsc_End();
        rt_thread_mdelay(10);
    }
}
int zino_gyro_task_init(void)
{
    rt_thread_t tid = rt_thread_create("gyro",
                                       zino_gyro_task,
                                       RT_NULL,
                                       2048,
                                       20,
                                       10);
    if (tid != RT_NULL)
    {
        LOG_I("create thread success");
        rt_thread_startup(tid);
        return RT_EOK;
    }
    else
    {
        LOG_E("create thread error");
        return -RT_ENOMEM;
    }
}
// ZINO_APP_EXPORT(zino_gyro_task_init);
